/*
 * MpfitLeastSquaresSolver.cpp
 *
 * Copyright 2012 Pieter Agten
 *
 * This file is part of tltgen.
 *
 * tltgen is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * tltgen is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with tltgen.  If not, see <http://www.gnu.org/licenses/>.
 */

/**
 * @file   MpfitLeastSquaresSolver.cpp
 * @author Pieter Agten (pieter.agten@gmail.com)
 * @date   15 jan 2013
 */

#include <stdexcept>
#include <iostream>
#include <cstring>
#include "mpfit.h"

#include "ThermistorTable.hpp"
#include "ThermistorModel.hpp"

#include "MpfitLeastSquaresSolver.hpp"

#define DEBUG false

namespace tltgen {

  MpfitLeastSquaresSolver::~MpfitLeastSquaresSolver()
  { }

  int MpfitLeastSquaresSolver::equation_fdf_wrapper(int N, int P, double *params, double *residues, double **derivs, void *data)
  {
    try {
      struct data* context = (struct data*)data;

      context->model->applyModelFunction(context->thermistorTable, params, residues);
      if (derivs) {
	double jacobian[N*P];

	context->model->applyModelDerivative(context->thermistorTable, params, jacobian);
	for (int j = 0; j < P; ++j) {
	  if (derivs[j]) {
	    for (int i = 0; i < N; ++i) {
	      derivs[j][i] = jacobian[i * P + j];
	    }
	  }
	}
      }
      return 0;
    } catch (std::exception& e) {
      return -1;
    }
  }

  void MpfitLeastSquaresSolver::calculateParameters(ThermistorModel* model, ThermistorTable* thermistorTable, double *result) const
  {
    if (! model->canBeSolved(thermistorTable)) {
      throw std::runtime_error("Generator is not ready to be solved.");
    }
    
    const int P = (int)model->getNbParameters();
    const int N = (int)thermistorTable->getNbRows();

    struct mp_config_struct config;
    config.ftol = 1E-10;
    config.xtol = 1E-10;
    config.gtol = 1E-10;
    config.epsfcn = MP_MACHEP0;
    config.stepfactor = 100.0;
    config.covtol = 1E-14;
    config.maxiter = (int)getMaxNbIterations();
    config.maxfev = 0;
    config.nprint = 1;
    config.douserscale = 0;
    config.nofinitecheck = 1;
    config.iterproc = 0;

    mp_result info;
    memset(&info, 0, sizeof(info));

    mp_par constraints[P];
    memset(&constraints[0], 0, sizeof(constraints));
    for (int i = 0; i < P; ++i) {
      if (model->isParameterFixed(i)) {
	constraints[i].fixed = 1;
      }
      if (model->hasParameterLowerBound(i)) {
	constraints[i].limited[0] = 1;
	constraints[i].limits[0] = model->getParameterLowerBound(i);
      }
      if (model->hasParameterUpperBound(i)) {
	constraints[i].limited[1] = 1;
	constraints[i].limits[1] = model->getParameterUpperBound(i);
      }
      constraints[i].side = 3;
      //constraints[i].side = 2;
      //constraints[i].deriv_debug = 1;
    }

    model->getInitialGuess(result);

    struct data context;
    context.model = model;
    context.thermistorTable = thermistorTable;

    /* Call the mpfit library */
    int status = mpfit(&equation_fdf_wrapper, N, P, result, constraints, &config, (void *)&context, &info);
    if (status < 0) {
      std::cout << "[MPFIT] mpfit returned error code " << status << std::endl;
      return;
    }

#if DEBUG
    std::cout << "[MPFIT] found parameters after " << info.niter << " iterations" << std::endl;
#endif
  }
}

